import * as math from "mathjs";
import { d2r } from "./Common";
import DH from "./DH";

function getTCP(theta, model='UR5'){
  switch(model) {
    case 'UR5':
      return urForward(theta, model);
    default:
      return null;
  }
}

function getDTCP(theta, model='UR5'){
  const rTheta = theta.map(d=>d2r(d));
  return getTCP(rTheta, model);
}

function urForward( theta=[0,0,0,0,0,0], model='UR5' ) {

  let dh;
  let dof = 6;

  switch(model){
    case 'UR5':
      dh = math.matrix(DH.UR5);
      dof = 6;
      break;
    default:
      break;
  }

  if( !dh ) return;
  if( theta.length !== dof ) return;

  // print(DH_UR5)
  const a = dh.subset(math.index(math.range(0,6), 1)).reshape([6]).toArray();
  const d = dh.subset(math.index(math.range(0,6), 2)).reshape([6]).toArray();
  let alp = dh.subset(math.index(math.range(0,6), 3)).reshape([6]).toArray();

  const T=[];
  
  for ( var i = 0; i < dof; i++ ) {
    T[i]=[ 
      [ Math.cos(theta[i]),  -Math.sin(theta[i])*Math.cos(alp[i]), Math.sin(theta[i])*Math.sin(alp[i]),  a[i]*Math.cos(theta[i])],
      [ Math.sin(theta[i]),  Math.cos(theta[i])*Math.cos(alp[i]),  -Math.cos(theta[i])*Math.sin(alp[i]), a[i]*Math.sin(theta[i])],
      [ 0,                   Math.sin(alp[i]),                     Math.cos(alp[i]),                     d[i]],
      [ 0,                   0,                                    0,                                    1]        
    ]
  }

  // console.log("T", T);
  let T06 = math.matrix(T[0]);
  for( var i = 1; i < dof; i ++ ) {
    T06 = math.multiply(T06, math.matrix(T[i]));
  }

  // console.log(T06);

  const x = T06.get([0, 3]);// T06[0][3] ;
  const y = T06.get([1, 3]);// T06[1][3] ;
  const z = T06.get([2, 3]);// T06[2][3] ;

  const r11 = T06.get([0, 0]);
  const r21 = T06.get([1, 0]);
  const r31 = T06.get([2, 0]);

  const r12 = T06.get([0, 1]);
  const r22 = T06.get([1, 1]);
  const r32 = T06.get([2, 1]);

  const r13 = T06.get([0, 2]);
  const r23 = T06.get([1, 2]);
  const r33 = T06.get([2, 2]);

  // const beta = Math.atan2(Math.sqrt(r11 * r11 + r21*r21), -r31);
  // const cb = Math.cos(beta);
  // const alpha = Math.atan2(r21/cb, r11/cb);
  // const gamma = Math.atan2(r33/cb, r32/cb);

  // console.log( rpy2rv( r2d(beta), r2d(gamma), r2d(alpha)) );

  const th = Math.acos((r11+r22+r33-1)/2);
  const sth = Math.sin(th);
  const kx = (r32-r23)/(2*sth);
  const ky = (r13-r31)/(2*sth);
  const kz = (r21-r12)/(2*sth);

  const rv = [(th*kx),(th*ky),(th*kz)];

  const pos=[x, y, z, rv[0], rv[1], rv[2] ];
  // console.log(pos);
  return pos;
}

export {
  getTCP,
  getDTCP,
}